function main0401

clc;clear all;close all;
HiddenUnitNum=7;
rand('state',sum(100*clock));
% ԭʼ���� 
d=[        240.5,     0,        0,        210,       0,        172.5];
a=[        0,    0,   256,   0,       0,       0];
alpha=[    0,  pi/2,        0,        pi/2,    -pi/2,        pi/2];
L(1)=Link('d',d(1),'a',a(1),'alpha',alpha(1)); 
L(2)=Link('d',d(2),'a',a(2),'alpha',alpha(2));
L(3)=Link('d',d(3),'a',a(3),'alpha',alpha(3));
L(4)=Link('d',d(4),'a',a(4),'alpha',alpha(4));
L(5)=Link('d',d(5),'a',a(5),'alpha',alpha(5));
L(6)=Link('d',d(6),'a',a(6),'alpha',alpha(6));
du=pi/180;
%����ؽڷ�Χ
L(1).qlim =[0,180]*du;
L(2).qlim =[0,180]*du;%-10,130
L(3).qlim =[0,180]*du;%-140,0
L(4).qlim =[0,180]*du;
L(5).qlim =[0,270]*du;
dofbot=SerialLink(L,'name','�����ɶ�Э����е��');
Theta=[0 0 0 0 0 0];
Theta=Theta/180*pi;
Theta2=[pi pi pi pi pi pi];
Theta2=Theta2/180*pi;   
forward1=dofbot.fkine(Theta);
forward2=dofbot.fkine(Theta2);
T1=forward1;
T2=forward2;
%��Ӧ�ؽڽ�
q1=dofbot.ikine(T1,[0 0 0 0 0 0],[1 1 1 1 1 1]);
q2=dofbot.ikine(T2,[0 0 0 0 0 0],[1 1 1 1 1 1]);
%q123=q1-Theta  Ӧ���Ƕ��ӽ���0
%q1234=q2-Theta2  Ӧ���Ƕ��ӽ���0
%��A�ƶ���B����ʱ6s��ÿ100ms����һ�ιؽڽ�
t=[0:0.1:60]';
%A�㵽B�㣬6���ؽڽ���6s�еı任���̡�v��a���ٶȺͼ��ٶȡ�
[q,v,a]=jtraj(q1,q2,t);
t1=q(:,1);
t2=q(:,2);
t3=q(:,3);
t4=q(:,4);
t5=q(:,5);
t6=q(:,6);
%����ѵ������.һ��ȷ��λ�ڹ����ռ��ڡ�
syms theta1 theta2 theta3 theta4 theta5 theta6

A1=[cos(theta1) -sin(theta1)*cos(alpha(1)) sin(theta1)*sin(alpha(1)) a(1)*cos(theta1);
    sin(theta1) cos(theta1)*cos(alpha(1)) -cos(theta1)*sin(alpha(1)) a(1)*sin(theta1);
    0 sin(alpha(1)) cos(alpha(1)) d(1);
    0 0 0 1];
A2=[cos(theta2) -sin(theta2)*cos(alpha(2)) sin(theta2)*sin(alpha(2)) a(2)*cos(theta2);
    sin(theta2) cos(theta2)*cos(alpha(2)) -cos(theta2)*sin(alpha(2)) a(2)*sin(theta2);
    0 sin(alpha(2)) cos(alpha(2)) d(2);
    0 0 0 1];
A3=[cos(theta3) -sin(theta3)*cos(alpha(3)) sin(theta3)*sin(alpha(3)) a(3)*cos(theta3);
    sin(theta3) cos(theta3)*cos(alpha(3)) -cos(theta3)*sin(alpha(3)) a(3)*sin(theta3);
    0 sin(alpha(3)) cos(alpha(3)) d(3);
    0 0 0 1];
A4=[cos(theta4) -sin(theta4)*cos(alpha(4)) sin(theta4)*sin(alpha(4)) a(4)*cos(theta4);
    sin(theta4) cos(theta4)*cos(alpha(4)) -cos(theta4)*sin(alpha(4)) a(4)*sin(theta4);
    0 sin(alpha(4)) cos(alpha(4)) d(4);
    0 0 0 1];
A5=[cos(theta5) -sin(theta5)*cos(alpha(5)) sin(theta5)*sin(alpha(5)) a(5)*cos(theta5);
    sin(theta5) cos(theta5)*cos(alpha(5)) -cos(theta5)*sin(alpha(5)) a(5)*sin(theta5);
    0 sin(alpha(5)) cos(alpha(5)) d(5);
    0 0 0 1];
A6=[cos(theta6) -sin(theta6)*cos(alpha(6)) sin(theta6)*sin(alpha(6)) a(6)*cos(theta6);
    sin(theta6) cos(theta6)*cos(alpha(6)) -cos(theta6)*sin(alpha(6)) a(6)*sin(theta6);
    0 sin(alpha(6)) cos(alpha(6)) d(6);
    0 0 0 1];
T06=simplify(A1*A2*A3*A4*A5*A6);
% nx=pretty(simplify(T06(1,1)))
% %nx=simplify(collect(T06(1,1)))
% ox=pretty(simplify(T06(1,2)))
% ax=pretty(simplify(T06(1,3)))
% ny=pretty(simplify(T06(2,1)))
% oy=pretty(simplify(T06(2,2)))
% ay=pretty(simplify(T06(2,3)))
% nz=pretty(simplify(T06(3,1)))
% oz=pretty(simplify(T06(3,2)))
% az=pretty(simplify(T06(3,3)))
% 
% px=simple(T06(1,4))
% py=simple(T06(2,4))
% pz=simple(T06(3,4))
Theta=[];
%�����������������
px=[];
py=[];
pz=[];
Tx=[];
Ty=[];
Tz=[];
[N,xxx]=size(q);%xxxΪ6
for i=1:N
    theta1=t1(i);
    theta2=t2(i);
    theta3=t3(i);
    theta4=t4(i);
    theta5=t5(i);
    theta6=t6(i);
    Lie=[theta1
        theta2
        theta3
        theta4
        theta5
        theta6];
    Theta = [Theta,Lie];
    T=eval(T06);
    px=[px
        T(1,4)];%Ŀ��������������
    py=[py
        T(2,4)];%Ŀ��������������
    pz=[pz
        T(3,4)];%Ŀ��������������
    R=T(1:3,1:3);
    r11=R(1,1);
    %r12=R(1,2);
    %r13=R(1,3);
    r21=R(2,1);
    %r22=R(2,2);
    %r23=R(2,3);
    r31=R(3,1);
    r32=R(3,2);
    r33=R(3,3);
    Tz=[Tz
        atan2(r21,r11)];%Ŀ��������������
    Ty=[Ty
        atan2(-r31,sqrt((r32^2)+(r33^2)))];%Ŀ��������������
    Tx=[Tx
        atan2(r32,r33)];%Ŀ��������������
end
                
%�������ݾ���
IN=[px py pz Tx Ty Tz]';
%Ŀ�����ݾ���
OUT=Theta;

% �������ݾ���
p=IN;  %6x601
 % Ŀ�����ݾ���
t=OUT;   %6X601
% ����mapminmax���������ݽ��й�һ��
[pn,input_str] = mapminmax(p) ;
[tn,output_str] = mapminmax(t) ;
TrainSamIn=pn;
TrainSamOut=tn;
[xxx1,TrainSamNum]=size(TrainSamIn);%xxx1Ϊ6�����������ά�ȣ��˶�ѧ�����е�δ֪����
[xxx2,TrainSamNum]=size(TrainSamOut);%xxx2Ϊ6�����������ά�ȣ�6���ؽڽǶ�
% for HiddenUnitNum=3:MaxHiddenLayerNode %��������Ԫ�ĸ�������ȡ������ĺ�������
    fprintf('\n the hidden layer node');HiddenUnitNum
    TrainNNOut=[];
    TestNNOut=[];
    %for t=1:MaxRunningTime
        %fprintf('the current running times is');t
        [NewW1,NewB1,NewW2,NewB2]=PSO_GATrain(TrainSamIn,TrainSamOut,HiddenUnitNum);
        disp('PSO-GA�㷨ѵ�������������LM�㷨����ѵ�����硭��');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%net=newff(minmax(pn),[HiddenUnitNum 6],{'logsig','purelin'},'traingdm');
net=newff(minmax(pn),[HiddenUnitNum 6],{'logsig','purelin'},'trainlm');
net.trainParam.show=10;
net.trainParam.lr=0.05; 
net.trainParam.epochs=50000;
%net.trainParam.epochs=20;
net.trainParam.goal=10^(-4);  
net.divideFcn = '';
%NewW1
%NewB1
%NewW2
%NewB2
net.iw{1,1}=NewW1;%7��6
net.b{1}=NewB1;%7��1
net.lw{2,1}=NewW2;%6��7
net.b{2}=NewB2;%6x1
tic
net=train(net,pn,tn);
toc%tic��toc���ڼ�¼ѵ��ʱ��
net.iw{1,1};
net.b{1};
net.lw{2,1};
net.b{2};
%�����������ݣ���������������������
% Theta3=[30 40 50 60 70 80];
% Theta3=Theta3/180*pi;                         %����ɻ���
% Theta4=[40 60 80 100 120 150];
% Theta4=Theta4/180*pi;                         %����ɻ���
% forward3=dofbot.fkine(Theta3);
% forward4=dofbot.fkine(Theta4);
T3=forward1;
T4=forward2;
%��Ӧ�ؽڽ�
q3=dofbot.ikine(T3,[0 0 0 0 0 0],[1 1 1 1 1 1]);
q4=dofbot.ikine(T4,[0 0 0 0 0 0],[1 1 1 1 1 1]);
%��A�ƶ���B����ʱ6s��ÿ100ms����һ�ιؽڽ�
t=[0:0.03:6]';
%A�㵽B�㣬6���ؽڽ���6s�еı任���̡�v��a���ٶȺͼ��ٶȡ�
[qtest,v,a]=jtraj(q3,q4,t);
t1test=qtest(:,1);
t2test=qtest(:,2);
t3test=qtest(:,3);
t4test=qtest(:,4);
t5test=qtest(:,5);
t6test=qtest(:,6);
Thetatest=[];
pxtest=[];
pytest=[];
pztest=[];
Txtest=[];
Tytest=[];
Tztest=[];
[Ntest,xxx]=size(qtest);%xxxΪ6
for i=1:Ntest
    theta1=t1test(i);
    theta2=t2test(i);
    theta3=t3test(i);
    theta4=t4test(i);
    theta5=t5test(i);
    theta6=t6test(i);
    Lie=[theta1
        theta2
        theta3
        theta4
        theta5
        theta6];
    Thetatest=[Thetatest,Lie];
    T=eval(T06);
    pxtest=[pxtest
        T(1,4)];%Ŀ��������������
    pytest=[pytest
        T(2,4)];%Ŀ��������������
    pztest=[pztest
        T(3,4)];%Ŀ��������������
    R=T(1:3,1:3);
    r11=R(1,1);
    %r12=R(1,2);
    %r13=R(1,3);
    r21=R(2,1);
    %r22=R(2,2);
    %r23=R(2,3);
    r31=R(3,1);
    r32=R(3,2);
    r33=R(3,3);
    Tztest=[Tztest
        atan2(r21,r11)];%Ŀ��������������
    Tytest=[Tytest
        atan2(-r31,sqrt((r32^2)+(r33^2)))];%Ŀ��������������
    Txtest=[Txtest
        atan2(r32,r33)];%Ŀ��������������
end
%�����������ݾ���
INtest=[pxtest pytest pztest Txtest Tytest Tztest]';
%����Ŀ�����ݾ���
OUTtest=Thetatest;
ptest=INtest;
ttest=OUTtest;
[pntest,input_str] = mapminmax(ptest) ;
[tntest,output_str] = mapminmax(ttest) ;
% ����ѵ���õ����磬���ڲ������ݶ�BP�������
antest=sim(net,pntest);
% ���ú���mapminmax�ѷ���õ������ݻ�ԭΪԭʼ��������
% �°汾�Ƽ�ѵ��������һ���ͷ���һ����ʹ�� mapminmax ����
a = mapminmax('reverse',antest,output_str);
% a��ʵ����ttest�����������ݽ��в���
%��ͼ����ʾ�������ܣ�ָ��ѡ����������������������Ĳ�ֵ�ľ���ֵ����������
x=1:1:Ntest;
Etheta1=abs(a(1,:)-ttest(1,:));                                     
Etheta2=abs(a(2,:)-ttest(2,:)); 
Etheta3=abs(a(3,:)-ttest(3,:));
Etheta4=abs(a(4,:)-ttest(4,:));
Etheta5=abs(a(5,:)-ttest(5,:));
Etheta6=abs(a(6,:)-ttest(6,:));
figure

subplot(2,1,1);plot(x,Etheta1,'r-o',x,Etheta2,'b--+',x,Etheta3,'y-.*') 
legend('�ؽڽǶ�1','�ؽڽǶ�2','�ؽڽǶ�3');
xlabel('����');ylabel('�������ֵ');
title('����������������˶�ѧ����������');
subplot(2,1,2);plot(x,Etheta4,'r-o',x,Etheta5,'b-.+')  
legend('�ؽڽǶ�4','�ؽڽǶ�5');
xlabel('����');ylabel('�������ֵ');
title('����������������˶�ѧ����������');
